#include "Calculator.h"

Calculator::Calculator(ros::NodeHandle& nh)
{
    act_command_publisher = nh.advertise<std_msgs::Float32MultiArray>("/uav_event/execution_res", 10);

    k_d = 0.5;
    k_i = 0.1;

    // nh.getParam("/k_d", k_d);
    // nh.getParam("/k_i", k_i);

    cout << "k_d: " << k_d << endl;
    cout << "k_i: " << k_i << endl;

    ans      = make_pair(0, 0);
    ans_last = make_pair(0, 0);
    force    = make_pair(0, 0);
}


void Calculator::feed(const std_msgs::Float64MultiArrayConstPtr& msg)
{
    // TODO
    // 校验msg信息是否正确
    if ( msg->data.size() == 0 ) {
        return;
    }
    // if ( msg->data[0] * 7 + 1 !=  msg->data.size()) {
    //
    // }

    int cnt = 0;

    // 障碍物数量
    n = msg->data[cnt++];

    ans = make_pair(0, 0);

    while ( n-- ) {
        id      = msg->data[cnt++];
        v_ruf.x = msg->data[cnt++] * 0.08;
        v_ruf.y = msg->data[cnt++] * 0.08;
        x       = msg->data[cnt++];
        y       = msg->data[cnt++];
        w       = msg->data[cnt++];
        h       = msg->data[cnt++];


        // 计算出每个球的躲避方向，用速度大小进行加权，然后合成
        count_each_ball();
    }

    // 计算出最终的躲避方向
    count_force();
}


// 计算出每个球的躲避方向并合成
void Calculator::count_each_ball()
{
    // 速度方向转化
    Vec3d v_flu(0, -v_ruf.x, v_ruf.y);

    ROS_INFO("v_flu: %f, %f", v_flu.y, v_flu.z);

    pair<double, double> v_vertical;

    // 找出朝上的方向向量
    if ( v_flu.y > 0 ) {
        v_vertical.first  = -v_flu.z;
        v_vertical.second = v_flu.y;
    }
    else {
        v_vertical.first  = v_flu.z;
        v_vertical.second = -v_flu.y;
    }

    ans.first += v_vertical.first;
    ans.second += v_vertical.second;
}


// 根据当前帧和之前累积帧的速度方向合成最终的速度方向
void Calculator::count_force()
{
    force.first  = k_d * ans.first + k_i * ans_last.first;
    force.second = k_d * ans.second + k_i * ans_last.second;
    ans_last     = ans;


    double force_len = sqrt(force.first * force.first + force.second * force.second);
    if ( force_len > 1 ) {
        // 如果输出强度过大，则进行归一化
        if ( force_len > 2 ) {
            force.first /= force_len;
            force.second /= force_len;
        }

        pub_force();
    }
    ROS_INFO("force: %f, %f", force.first, force.second);
}


// 发布act话题
void Calculator::pub_force()
{
    std_msgs::Float32MultiArray msg;
    msg.data.push_back(1);
    msg.data.push_back(0);              // x轴
    msg.data.push_back(force.first);    // y轴
    msg.data.push_back(force.second);   // z轴

    act_command_publisher.publish(msg);

    // 清除计算值
    force = {0,0};
    ans   = {0,0};
    ans_last = {0,0};
}